/*
TP Peripherique
Copyright 2009 Boris Maquin, Paul Cox 

File: main.c
Version: 1
*/
#include <intrins.h>	// pour la fonction void _nop_(void)
#include <reg167.h>
#include <stdio.h>	
#include "../lib/moteur_plateau.h"
#include "../lib/radio_command.h"
#include "../lib/swuart.h"

void init_fonction();

int speed1;
int dir1;


static int vit_eff=0;

#define MOT_SEUIL  (10)

int abs(int val) {
	if (val < 0) {
		return -val;
	}
	return val;
}

void moteur_slew()  {
	int vit_des,dif;

	vit_des = get_speed();
	dif = vit_des - vit_eff;
	if ( abs(dif) < MOT_SEUIL ){
		vit_eff = vit_des;
	} else {
		if (dif > 0) {
			dif = MOT_SEUIL;
		} else {
			dif = -MOT_SEUIL;
		}
	vit_eff += dif;
	}
	
	gerer_moteur_plateau(vit_eff,get_direction());
}

void Attendons1 (long int Duree)	{

	long Compte;

	Compte = Duree ;
	while (Compte > 0 ){
		_nop_();
		Compte = Compte-1;
	}
}
void main() {

	//char* pchar = "o";

 	init_fonction();
	IEN = 1;
	//PSW = PSW | 0x0800;   // autorisation globale des interruptions
			
	//printf("hello \n");
	while (1){
//	speed1=get_speed();
//	dir1=get_direction();
		//printf("Speed: %d Dir: %d\r\n",get_speed() , get_direction());
//		gerer_moteur_plateau ( get_speed() , get_direction() );
		moteur_slew();
		Attendons1(0x554);
		 //put_char1('5');
		_nop_();
	
	}
} 

void init_fonction() {
	init_swuart();
	init_rc();
	init_moteur_plateau();
	T2CON = 0x00;

}






				